//
// Created by tony3 on 10/23/2018.
//
#include <stdio.h>


struct pid_controller{
    float kp,ki,kd;
    int target,current;
    int error;
    int integral;
    int last_error;
    int output;
};

 struct pid_controller pid_controller1;


 void pid_set_param(struct pid_controller *pid_obj, float kp, float ki, float kd) {
    pid_obj->kp=kp;
    pid_obj->ki=ki;
    pid_obj->kd=kd;
}

void pid_init(struct pid_controller *pid_obj) {
    pid_obj->integral=0;
    pid_obj->last_error=0;
}

void pid_cal(struct pid_controller *pid_obj,int target) {
    float output;
    pid_obj->target=target;
    pid_obj->error=pid_obj->target-pid_obj->current;
    pid_obj->integral+=pid_obj->error;
    pid_obj->output=pid_obj->kp*pid_obj->error+pid_obj->ki*pid_obj->integral+pid_obj->kd*(pid_obj->error-pid_obj->last_error);
    pid_obj->last_error=pid_obj->error; //update error
}




